#include "mlSphere.h"
#include <math.h>
#include <stdio.h>
#include <string.h>

void
mlSpherePrint(const float *a)
{
	printf("[(%f, %f, %f), %f]\n", a[0], a[1], a[2], a[3]);
}

void
mlSphereCopy(const float *a, float *result)
{
	memcpy(result, a, 4 * sizeof(float));
}

void
mlSphereLoad(float *a, float x, float y, float z, float r)
{
	a[0] = x; a[1] = y; a[2] = z; a[3] = r;
}

void
mlSphereMergeTo(const float *a, const float *b, float *result)
{
	float distancer[3], distance;

	distancer[0] = b[0] - a[0];
	distancer[1] = b[1] - a[1];
	distancer[2] = b[2] - a[2];

	distance = sqrtf(powf(distancer[0], 2) + powf(distancer[1], 2) + powf(distancer[2], 2));

	if(distance + b[3] > a[3])
	{
		if(distance + a[3] > b[3])
		{
			distancer[0] /= distance;
			distancer[1] /= distance;
			distancer[2] /= distance;

			result[3] = (distance + a[3] + b[3]) / 2.0f;
			result[0] = a[0] + distancer[0] * (result[3] - a[3]);
			result[1] = a[1] + distancer[1] * (result[3] - a[3]);
			result[2] = a[2] + distancer[2] * (result[3] - a[3]);
		}
		else
			mlSphereCopy(b, result);
	}
	else
		mlSphereCopy(a, result);
}

void
mlSphereMergeInPlace(float *a, const float *b)
{
	float result[4];

	mlSphereMergeTo(a, b, result);
	mlSphereCopy(result, a);
}
